-- 描述: 获取机器人当前的笛卡尔坐标以及关节坐标
-- 输入: 无
-- 输出: 无
function GetRbtLoc()
	
	-- 获取机器人当前笛卡尔坐标
	CurrentLoc = Location.New()
	CurrentLoc:Here(1)
	
	-- 将当前笛卡尔坐标信息写入共享变量
	GDouble[5] =  math.floor(CurrentLoc.X*1000+0.5)
	GDouble[6] =  math.floor(CurrentLoc.Y*1000+0.5)
	GDouble[7] =  math.floor(CurrentLoc.Z*1000+0.5)
	GDouble[8] =  math.floor(CurrentLoc.Roll*1000+0.5)
	
	-- 获取机器人当前关节坐标
	CurrentLocJ = LocationJ.New()
	CurrentLocJ:Here(1)
	
	-- 将当前关节坐标信息写入共享变量
	GDouble[9] =  math.floor(CurrentLocJ.J1*1000+0.5)
	GDouble[10] =  math.floor(CurrentLocJ.J2*1000+0.5)
	GDouble[11] =  math.floor(CurrentLocJ.J3*1000+0.5)
	GDouble[12] = math.floor(CurrentLocJ.J4*1000+0.5)
	
end

-- 描述: 获取机器人当前的上伺服状态、回零状态以及急停状态
-- 输入: 无
-- 输出: 无
function GetRbtStt()
	
	-- 获取机器人的上伺服以及回零的状态
	Bool_Power, Bool_Home, Bool_RunState,Bool_EStop = Robot.State(1)
	
	if Bool_Power == 1 then
		-- 将当前上伺服状态写入共享变量
		GBool[22] = true
	else
		-- 将当前上伺服状态写入共享变量
		GBool[22] = false
	end
	
	if Bool_EStop == 1 then
		
		-- 根据当前急停状态修改输出状态
		if (IO.Get(DO_ESTOP) == 0) then
			IO.Set(DO_ESTOP,1)
		end
		
	else
		
		-- 根据当前急停状态修改输出状态
		if (IO.Get(DO_ESTOP) == 1) then
			IO.Set(DO_ESTOP,0)
		end
		
	end
	
end

-- 描述: 设置机器人当前的速度
-- 输入: 无
-- 输出: 无
function SetRbtSpeed()
	
	-- 读取共享变量
	ReadSpeed = GDouble[27]
	
	if ReadSpeed > 100 then
		ReadSpeed = 100
	elseif ReadSpeed <= 0 then
		ReadSpeed = 1
	end 
	
	if RbtSpeed ~= ReadSpeed then
		System.Speed(ReadSpeed)
		RbtSpeed = ReadSpeed
		-- 将当前速度写入共享变量
		GDouble[2] = RbtSpeed
	end

end

-- 描述: 实时更新机器人相关的状态
-- 输入: 无
-- 输出: 无
function UpdateRbtStt()
	
	print("UpdateRbtStt Thread Start!!!")
	
	ReadSpeed = 0
	RbtSpeed = 0
	
	while true do
	
		-- 获取当前的笛卡尔坐标以及关节坐标(实时更新)
		GetRbtLoc()
		
		-- 获取当前的上伺服状态、回零状态以及急停状态(实时更新)
		GetRbtStt()
		
		-- 设置当前的系统速度(实时更新)
		SetRbtSpeed()
		
		Thread.Sleep(0.05)
		
	end
	
end